/*
 * MIAU.cpp
 *
 *  Created on: Mar 13, 2010
 *      Author: As_You_Wish
 */

#include "MIAU.h"
#include "global_defs.h"
extern "C" {
#include "SoR_Utils.h"
#include "timer640.h"
#include "rprintf.h"
#include <avr/io.h>
#include "Time.h"
#include "a2d.h"
}

MIAU::MIAU() {
	robot = new Robot(new SimpleReactiveBrain());

}

MIAU::~MIAU() {
	delete (robot);
}

void MIAU::init() {

	//add 1.7s delay for potential power issues
	delay_cycles(65535);
	delay_cycles(65535);
	delay_cycles(65535);
	delay_cycles(65535);
	delay_cycles(65535);
	delay_cycles(65535);
	delay_cycles(65535);

	/****************INITIALIZATIONS*******************/
	//other stuff Im experimenting with for SoR
	uartInit(); // initialize the UART (serial port)
	//    uartSetBaudRate(0, 38400); // set UARTE speed, for Bluetooth
	uartSetBaudRate(1, 115200); // set UARTD speed, for USB connection, up to 500k, try 115200 if it doesn't work
	//   uartSetBaudRate(2, 38400); // set UARTH speed
	//   uartSetBaudRate(3, 38400); // set UARTJ speed, for Blackfin
	//G=Ground, T=Tx (connect to external Rx), R=Rx (connect to external Tx)

	rprintfInit(uart1SendByte);// initialize rprintf system and configure uart1 (USB) for rprintf

	configure_ports(); // configure which ports are analog, digital, etc.

	LED_on();

	LOG("\r\nSystem Warmed Up");

	// initialize the timer system
	init_timer0(TIMER_CLK_1024);
	init_timer1(TIMER_CLK_64);
	init_timer2(TIMER2_CLK_64);
	init_timer3(TIMER_CLK_64);
	init_timer4(TIMER_CLK_64);
	init_timer5(TIMER_CLK_64);

	a2dInit(); // initialize analog to digital converter (ADC)
	a2dSetPrescaler(ADC_PRESCALE_DIV32); // configure ADC scaling
	a2dSetReference(ADC_REFERENCE_AVCC); // configure ADC reference voltage

	//	wdt_enable(WDTO_8S);

	//let system stabelize for X time
	for (int i = 0; i < 16; i++) {
		a2dConvert8bit(i);//read each ADC once to get it working accurately
		delay_cycles(5000); //keep LED on long enough to see Axon reseting
		LOG(".");
	}

	LED_off();

	// Set up compare mode interrupts
	OCR1A = 250; // It will trigger interrupt every 1ms
	// To make the FATFS library to work,
	// function disk_timerproc must be called every 10ms
	//	timer_attach(TIMER1_COMPA_interrupt,pulse);
	//	timer_attach(TIMER1_COMPA_interrupt,disk_timerproc);

	sei();
	// Enable global interrupts


	LOG("Initialization Complete \r\n");

	/**************************************************/

	//reset all timers to zero
	reset_timer0();
	reset_timer1();
	reset_timer2();
	reset_timer3();
	reset_timer4();
	reset_timer5();

}
int c=0;
void MIAU::begin() {

	while (true) {
		c++;
		if (c % 10 == 0) {
//			LOG(".");

			for (int i=0;i<NUM_SENSORS; i++) {
				DEBUG("%d ", robot->sensors[i]->readData());
			}
			DEBUG("\n");

		}
		robot->update();
		Time::sleep_ms(20);
	}
}

